#include "lidar_localization/models/graph_optimizer/g2o/edge/edge_imu.hpp"

namespace g2o
{
    EdgeImu::EdgeImu() : BaseMultiEdge<15, std::shared_ptr<lidar_localization::PreIntegration>>()
    {
        resize(10);
        // information().setIdentity();
        // setMeasurement(nullptr);
    }
    void EdgeImu::computeError()
    {
        VertexVec *Pi = dynamic_cast<VertexVec *>(_vertices[0]);
        VertexQ *Qi = dynamic_cast<VertexQ *>(_vertices[1]);
        VertexVec *Vi = dynamic_cast<VertexVec *>(_vertices[2]);
        VertexVec *Bai = dynamic_cast<VertexVec *>(_vertices[3]);
        VertexVec *Bgi = dynamic_cast<VertexVec *>(_vertices[4]);
        VertexVec *Pj = dynamic_cast<VertexVec *>(_vertices[5]);
        VertexQ *Qj = dynamic_cast<VertexQ *>(_vertices[6]);
        VertexVec *Vj = dynamic_cast<VertexVec *>(_vertices[7]);
        VertexVec *Baj = dynamic_cast<VertexVec *>(_vertices[8]);
        VertexVec *Bgj = dynamic_cast<VertexVec *>(_vertices[9]);
        // if ((Bai->estimate() - _measurement->linearized_ba_).norm() > 0.1 ||(Bgi->estimate() - _measurement->linearized_bg_).norm() > 0.01)
        // {
        //     _measurement->repropagate(Bai->estimate(), Bgi->estimate());
        //     _information = _measurement->covariance_.inverse();
        // }
        _error = _measurement->evaluate(Pi->estimate(), Qi->estimate(), Vi->estimate(), Bai->estimate(), Bgi->estimate(), Pj->estimate(), Qj->estimate(), Vj->estimate(), Baj->estimate(), Bgj->estimate());
    }
    void EdgeImu::linearizeOplus()
    {
        VertexVec *Pi = dynamic_cast<VertexVec *>(_vertices[0]);
        VertexQ *Qi = dynamic_cast<VertexQ *>(_vertices[1]);
        VertexVec *Vi = dynamic_cast<VertexVec *>(_vertices[2]);
        VertexVec *Bai = dynamic_cast<VertexVec *>(_vertices[3]);
        VertexVec *Bgi = dynamic_cast<VertexVec *>(_vertices[4]);
        VertexVec *Pj = dynamic_cast<VertexVec *>(_vertices[5]);
        VertexQ *Qj = dynamic_cast<VertexQ *>(_vertices[6]);
        VertexVec *Vj = dynamic_cast<VertexVec *>(_vertices[7]);
        VertexVec *Baj = dynamic_cast<VertexVec *>(_vertices[8]);
        VertexVec *Bgj = dynamic_cast<VertexVec *>(_vertices[9]);
        // if ((Bai->estimate() - _measurement->linearized_ba_).norm() > 0.1 ||(Bgi->estimate() - _measurement->linearized_bg_).norm() > 0.01)
        // {
        //     _measurement->repropagate(Bai->estimate(), Bgi->estimate());
        //     _information = _measurement->covariance_.inverse();
        // }
        Eigen::Matrix<double, 15, 30> jacs = _measurement->jacobian(Pi->estimate(), Qi->estimate(), Vi->estimate(), Bai->estimate(), Bgi->estimate(), Pj->estimate(), Qj->estimate(), Vj->estimate(), Baj->estimate(), Bgj->estimate());
        for (unsigned int i = 0; i < _vertices.size(); i++)
        {
            _jacobianOplus[i] = jacs.block<15, 3>(0, 3 * i);
        }
    }
    void EdgeImu::setMeasurement(const std::shared_ptr<lidar_localization::PreIntegration> &m)
    {
        _measurement = m;
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 15, 15>> saes(_measurement->covariance_);
        double eps = 1e-8;
        _information = saes.eigenvectors() * Eigen::VectorXd((saes.eigenvalues().array() > eps).select(saes.eigenvalues().array().inverse(), 0)).asDiagonal() * saes.eigenvectors().transpose();
    }
} // namespace g2o